/*********************************************************************
 * RocRailuino
 * Created 06 nov 2013 Bert Havinga
 *
 * This sketch runs on Arduino Uno in conjuction with a CAN bus
 * shield http://www.watterott.com/en/Arduino-CANdiy-Shield
 * this shield is a development as a part of the Railuino project.
 * Rocrail should be configured to communicate with a Maerklin CS2
 * Optional is pin 9 configured to monitor a shortcircuit signal
 * If connected to a CS2 or a GFP 60113 please comment it out
 *
 * Based on Railuino library. The library is stripped for use
 * in combination with RocRail©. The original Railuino library
 * didn't support the priobits in the CANheader.
 * 
 * excerpt from Railuino - Hacking your Märklin
 * Idea
 * Copyright (C) 2012 Joerg Pleumann
 * 
 * This example is free software; you can redistribute it and/or
 * modify it under the terms of the Creative Commons Zero License,
 * version 1.0, as published by the Creative Commons Organisation.
 * This effectively puts the file into the public domain.
 *
 * This example is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * LICENSE file for more details.
 */

#include <Railuino.h>
#include <SoftwareSerial.h>

TrackController ctrl(0x0000, false);

TrackMessage message;

// RocRail sends always 13 bytes
char sbuffer[13];
byte rbuffer[13];
int inPin = 9;   // shortcircuit monitorsignal connected to digital pin 9

int i;

void setup() {
  Serial.begin(500000);
  while (!Serial);
  ctrl.begin();
  Serial.setTimeout(20);
  pinMode(inPin, INPUT);      // sets the digital pin 7 as input
}

void loop() {
// check for shortcircuit signal from booster
  if (digitalRead(inPin) == LOW ) {
    ctrl.setPower(false);
  }
  if (ctrl.receiveMessage(message)) {
    rbuffer[0] = ((message.command >> 7) & 0x1F);  // save prio and Command msbit
    rbuffer[1] = ((message.command  & 0x7F) << 1);
    rbuffer[1] |= (message.response & 0x01);
    rbuffer[2] = ((message.hash >> 8) & 0x00ff);
    rbuffer[3] = (message.hash & 0x00ff);
    rbuffer[4] = message.length;
    for (int i = 0; i < message.length; i++) {
      rbuffer[i+5] = message.data[i];
    }
    for (int i=8; i> message.length; i--) {
      rbuffer[i+4] = 0;
    }  
    Serial.write(rbuffer, 13);
  }

  if (Serial.available()) {
      if (Serial.readBytes(sbuffer, 13) == 13) {
      message.clear();
//      message.command = (sbuffer[0] << 7);
      message.command = (sbuffer[0] << 7);  // shift prio and command msbit
      message.response = (sbuffer[1] & 0x01);
      message.command |= ((sbuffer[1] & 0xFE) >> 1);
      message.hash = ctrl.getHash();
      message.length = sbuffer[4];
      for (int i=0; i < 9; i++) {
         message.data[i] = sbuffer[i+5];
      }  
      ctrl.sendMessage(message);
    }
  }
}

